#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>

// PID控制器参数
double Kp = 0.5;
double Ki = 0.1;
double Kd = 0.05;

// 控制器输出限制
double max_vel = 1.0;
double min_vel = -1.0;

// 控制器状态变量
double error = 0.0;
double error_integral = 0.0;
double error_derivative = 0.0;
double last_error = 0.0;

// 目标位置
double target_x = 0.0;
double target_y = 0.0;

// 当前位置
double current_x = 0.0;
double current_y = 0.0;

// 订阅机器人当前位置
void odomCallback(const nav_msgs::Odometry::ConstPtr &msg) {
    current_x = msg->pose.pose.position.x;
    current_y = msg->pose.pose.position.y;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "cre_pid_controller_node");
    ros::NodeHandle nh;

    // 创建一个Publisher，发布控制指令
    ros::Publisher cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);

    // 创建一个Subscriber，订阅机器人当前位置
    ros::Subscriber odom_sub = nh.subscribe("odom", 1, odomCallback);

    // 设置控制频率
    ros::Rate rate(50);

    while (ros::ok()) {
        // 计算位置误差
        error = target_x - current_x;

        // 计算误差积分项
        error_integral += error;

        // 计算误差微分项
        error_derivative = error - last_error;

        // 计算控制指令
        double cmd_vel = Kp * error + Ki * error_integral + Kd * error_derivative;

        // 限制控制指令范围
        if (cmd_vel > max_vel) {
            cmd_vel = max_vel;
        } else if (cmd_vel < min_vel) {
            cmd_vel = min_vel;
        }

        // 发布控制指令
        geometry_msgs::Twist cmd_vel_msg;
        cmd_vel_msg.linear.x = cmd_vel;
        cmd_vel_pub.publish(cmd_vel_msg);

        // 记录上一次误差
        last_error = error;

        // 等待下一个控制周期
        rate.sleep();
    }

    return 0;
}